#ifndef Z1ARM_H
#define Z1ARM_H

#include "unitree_arm_sdk/control/unitreeArm.h"
#include "unitree_arm_sdk/thirdparty/robotics.h"
#include <iostream>

class Z1ARM : public unitreeArm {
public:
    Z1ARM();
    ~Z1ARM();

    void armCtrlByFSM();
    void armCtrlInJointCtrl();
    void armCtrlInCartesian();
    void printState();

    double gripper_pos;
    double joint_speed;
    double cartesian_speed;

    void setForwardPosition(const UNITREE_ARM::Vec6& pos) { forward_position = pos; }
    UNITREE_ARM::Vec6 getForwardPosition() const { return forward_position; }

    int tcp_sockfd;

private:
    UNITREE_ARM::Vec6 forward_position;
};

#endif // Z1ARM_H
